/*
 * @Author: _oufen
 * @Date: 2023-07-11 20:23:19
 * @LastEditTime: 2023-07-19 11:10:13
 * @Description:  主要控制模块
 */
/**
 *************************************************************************************************************************
 * @file    control.c
 * @author  oufen
 * @version V1.0
 * @date    2023-07-10
 * @brief   xx模块.c文件配置
 *************************************************************************************************************************
 * @attention
 *
 *
 *************************************************************************************************************************
 */

/* Includes -------------------------------------------------------------------------------------------------------------*/
#include "mycontrol.h"
#include "pid.h"
#include "encoder.h"
#include "motor.h"
#include "Tracking.h"
#include "stdio.h"
#include "string.h"
#include "oled.h"


/* 定义 -----------------------------------------------------------------------------------------------------------------*/

Param_InitTypedef Param;
Flag_InitTypedef Flag;

float speed1, speed2; // 测速
extern int track;
char wifi_data[30] = {0}; //发送给串口的数据
char a[30] = {0}; //发送给串口的数据
int z = 10;
int cnt = 0;

void K210_Sendstring(char *str)
{
    while(*str != '\0')
    {
        DL_UART_Main_transmitDataBlocking(UART_3_INST,*str++);
    }
}

void WI_SendString(char * str)
{
    while(*str != '\0')
    {
        DL_UART_Main_transmitDataBlocking(UART_1_INST,*str++);
    }
}

void HMI_Sendstring(char *str)
{
    while(*str != '\0')
    {
        DL_UART_Main_transmitDataBlocking(UART_1_INST,*str++);
    }
}

void TIMER_0_INST_IRQHandler(void) // 5ms主程序循环
{
  // uint8_t flag1 = 0;
    // static uint32_t cnt = 0;

    //  cnt++; // 5ms进入一次中断

    //  if (cnt >= 20) // 5ms   *200   1s一次进入
    //  {
    //  }
    
    // 获取单位时间内脉冲数
    Param.UnitTime_Motor1Pluse = Encoder_Get(1);
    Param.UnitTime_Motor2Pluse = Encoder_Get(2);
    // printf("encoder1 = %d,encoder2 = %d\r\n", Param.UnitTime_Motor1Pluse, Param.UnitTime_Motor2Pluse);

    // 获取累积脉冲数
    Param.Sigma_Motor1_Pluse += Param.UnitTime_Motor1Pluse;
    Param.Sigma_Motor2_Pluse += Param.UnitTime_Motor2Pluse;

    // 获取rpm  和滤波后的速度      注意这里是二倍频了 不是四倍频  rpm  = 单位时间脉冲*1000*60  / 13*2*20*5
    // 注意在强制类型转换时  小数后面添加f  防止程序出现warring
    // warring: #1035-D: single-precision operand implicitly converted to double-precision      
    speed1 = (float)Param.UnitTime_Motor1Pluse * 0.038667f; // MG-310 m/s
    speed2 = (float)Param.UnitTime_Motor2Pluse * 0.038667f; // MG-310 m/s
    //printf("main.sp1.txt=%.1f\xff\xff\xff",Pid_speed1);
    // printf("speed1 = %f,speed2 = %f\r\n", speed1, speed2);
    //sprintf(a,"speed1:%.1f\r\n",Pid_speed1);
    // printf("n0.val=%.1f\xff\xff\xff",Pid_speed1);
    // printf("n2.val=%.1f\xff\xff\xff",Pid_speed2);
    

    // 期望rpm
    pid_speed1.target_val = 30.0;//目标速度
    pid_speed2.target_val = 30.0;
    // Param.Motor1_PWM_Out = speed1_pid_control(speed1);
    // Param.Motor1_PWM_Out = speed1_pid_control(Param.UnitTime_Motor1Pluse); 自写速度环

    // 速度环
    Param.Motor1_PWM_Out = speed1_pid_control();
    Param.Motor2_PWM_Out = speed2_pid_control();

    // printf("motor1 = %d,motor2 = %d\r\n", Param.Motor1_PWM_Out, Param.Motor2_PWM_Out);  操蛋  会冲突

    if (Param.Motor1_PWM_Out > 0)
    {
        Motor_Left_DIR(FORWARD);
    }
    //  else
    //  {
    //      Motor_Left_DIR(BACK);
    //  }

    if (Param.Motor2_PWM_Out > 0)
    {
        Motor_Right_DIR(FORWARD);
    }
    //  else
    //  {
    //      Motor_Right_DIR(BACK);
    //  }

    //    // 舵机巡线pid,SERVO_MID_CCR表示中值舵机PWM波

    if (Pid_location > 0) // 说明往左偏
    {
        //Param.Line_Out = SERVO_MID_CCR - location_pid_control(); // 舵机巡线环输出函数
        Motor_Left_DIR(FORWARD);
       Load_Motor_Pwm(2000+location_pid_control(), 2000-location_pid_control());  
    }
    else if (Pid_location < 0) // 说明往右偏
    {
        //Param.Line_Out = SERVO_MID_CCR + location_pid_control(); // 舵机巡线环输出函数
        Motor_Left_DIR(FORWARD);
        Load_Motor_Pwm(2000+location_pid_control(), 2000-location_pid_control());  //location_pid_control()
    }
    else
    {
       Load_Motor_Pwm(2000, 2000);  //location_pid_control()
    }

    //if (Param.Line_Out > SERVO_MAX) // 对舵机进行限幅
    //{
    //    Param.Line_Out = SERVO_MAX;
    //}
    //else if (Param.Line_Out < SERVO_MIN)
    //{
     //   Param.Line_Out = SERVO_MIN;
    //}

    //输出舵机转弯值
    //DL_TimerG_setCaptureCompareValue(DUOJI_PWM_INST, 5000 ,DL_TIMER_CC_0_INDEX);//Param.Line_Out
    //电机装载输出值
    //Load_Motor_Pwm(2000, 2000);
}
